import time

import numpy as np
b=np.loadtxt('./draw_trajectory/KeyFrameTrajectory.txt',dtype=np.float32)
print(b)
t_time=0
rotation_matrix_y = np.asarray(
    [[1, 0, 0], [0, 1, 0], [0, 0,1]],dtype="float32")
for i in b:
    # print(t_time)
    # R_matrix = np.asarray([[i[1],0,0], [0,i[2],0], [0,0,i[3]]], dtype="float32")
    # t_matrix = np.asarray([[i[4],0,0], [0,i[5],0], [0,0,i[6]]], dtype="float32")
    # rotation_matrix_y = np.matmul(rotation_matrix_y, R_matrix)+t_matrix
    print("--------------------------------------------------------")
    print(rotation_matrix_y)
    # print("R：", R_matrix)
    # print("t：", t_matrix)
    rotation_matrix_y[-1, 0] += i[1]
    rotation_matrix_y[-1, 1] += i[2]
    rotation_matrix_y[-1, 2] += i[3]
    print("--------------------------------------------------------")
    print(rotation_matrix_y)